/***************************************************************************
 创建者: 华磊
 开始时间: 2017.6.28
 copyright: (C) 华友高科
 修改说明:
 ***************************************************************************
 *  @file axisgroup.h
 *  功能:
 * 多轴对象的管理和对外接口。
 *
 *                                                                         *
 ***************************************************************************/
#include "axisgroup.h"
#include "qdebug.h"
#include "AxisDefines.h"
#define D_USE_VELCOMPENSATION 1
#define D_MIX_ENLARGE_RATIO 1.3
AxisGroup::AxisGroup(int robotIdIn, RobotCommonParameter *roboParameterIn,
                     QVector<JointAttribute>  axisGroupAttributeIn,
                     QVector<JointAttribute> axisGroupCoordinateAttributeIn
                     , int groupId, double deltaTimeIn, int robotTypeIn,
                     MotionMessage *motionMessageIn, CoordinateManager *coordinateManagerIn)
{
    coordinateManager=coordinateManagerIn;
    robotId=robotIdIn;
    motionMessage=motionMessageIn;
    axisGroupAttribute= axisGroupAttributeIn;
    axisGroupCoordinateAttribute= axisGroupCoordinateAttributeIn;
    //所有关节轴初始化
    totalAxisNumber=axisGroupAttribute.size();
    armControlAxisNumber=roboParameterIn->armControlAxisNumber;
//    externAxisNumber=roboParameterIn->externalNum;
    agvAxisNum=roboParameterIn->agvAxisNum;
    virtualAxisNum=roboParameterIn->virtualAxisNum;
    targetWaitTime=roboParameterIn->targetWaitTime;

    errorStatusFlag=0;
    for(int i=0;i<totalAxisNumber;i++)
    {
        createAxisObject(i,axisGroupAttribute[i]);

    }

    //所有坐标轴初始化
    coordinateAxisNumber=1;
    for(int i=0;i<coordinateAxisNumber;i++)
    {
        createCoordinateAxisObject(10000+i,deltaTimeIn,axisGroupCoordinateAttribute[i],
                                   roboParameterIn->rotate_eqradius);
    }

    initialTimeAxis();

    axisGroupId=groupId;
    setDeltaTime(deltaTimeIn);
    currentRunningLine=-1;
    iscurrentAxisCommandFInished=-1;
//    currentCommandRemainingTime=0;
    totalRemainingTime=0;
    basicStatus=E_AXIS_STATUS_NORMAL;
    ceaseTimeDuration=100;

    robotType=robotTypeIn;

    axisGroupDecouple=new JointDecouple(&axisGroupAttribute,robotType,robotId,deltaTimeIn);

    QString tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
    QString xmlfile = tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+ZERO_CONFIG_FILE_NAME;
    zeroManagement=new ZeroManagement(xmlfile,robotType,robotId,totalAxisNumber,motionMessage,axisGroupDecouple);
    debugInfo.stage=E_AXISGROUP_STAGE_INIT;


}

int AxisGroup::axisSimpleLoopOnce()
{
    for(int i=0;i<axisArry.size();i++)
    {
          axisArry[i]->axisSimpleLoopOnce();
    }
    return 1;
}

int AxisGroup::getAxisSimpleTpInfo(int axisId, SimpleTpInfo &infoOut)
{
    if(axisId<0||axisId>=axisArry.size())
    {
        return -1;
    }
    axisArry[axisId]->getAxisSimpleTpInfo(infoOut);
    return 1;
}

int AxisGroup::controlAxisSimpleTp(int axisId, int isEnable)
{
    if(axisId<0||axisId>=axisArry.size())
    {
        return -1;
    }
    axisArry[axisId]->controlAxisSimpleTp(isEnable);
    return 1;
}

int AxisGroup::setAxisSimpleTpTarget(int axisId, double targetPosIn, double maxVelIn, double maxAccIn)
{
    if(axisId<0||axisId>=axisArry.size())
    {
        return -1;
    }
    axisArry[axisId]->setAxisSimpleTpTarget(targetPosIn,maxVelIn,maxAccIn);
    return 1;
}

E_AXIS_PLAN_MODE AxisGroup::getAxisPlanMode(int axisId)
{
    if(axisId<0||axisId>=axisArry.size())
    {
        return E_AXIS_PLAN_MODE_ERROR;
    }
    return axisArry[axisId]->getAxisPlanMode();
}

int AxisGroup::setAxisPlanMode(int axisId, E_AXIS_PLAN_MODE modeIn)
{
    if(axisId<0||axisId>=axisArry.size())
    {
        return -1;
    }
    axisArry[axisId]->setAxisPlanMode(modeIn);
    return 1;
}

int AxisGroup::getIdealTorque(QVector<double> &torqueOut)
{
    torqueOut.clear();
    for(int i=0;i<totalAxisNumber;i++)
    {
        torqueOut.append(axisArry[i]->getIdealTorque());
    }
    return 1;
}

int AxisGroup::setIdealTorque(QVector<double> torqueIn)
{
    for(int i=0;i<torqueIn.size();i++)
    {
        if(i<axisArry.size())
        {
            axisArry[i]->setIdealTorque(torqueIn[i]);
        }

    }
    return 1;
}

bool AxisGroup::isInDecartTrajStatus()
{
    return isInDecartTraj;
}

int AxisGroup::getErrorStatusFlag()
{
    return errorStatusFlag;
}

int AxisGroup::clearAllMsg()
{
    errorStatusFlag=0;
    return 1;
}

int AxisGroup::setRobotCoordinateConfig(RobotCoordinateAxisConfig coordinateConfigIn)
{
    targetWaitTime=coordinateConfigIn.targetWaitTime;
    if(coordinateAxisArry.size()>0)
    {
        coordinateAxisArry[0]->setRobotCoordinateConfig(coordinateConfigIn);
    }
    for(int i=0;i<totalAxisNumber;i++)
    {
        axisArry[i]->setT1Ratio(coordinateConfigIn.t1RatioLimit);
    }

    return 1;
}

float AxisGroup::getRunningNumber()
{
    return currentLine;
}

int AxisGroup::setRunningNumber_robotArmControl(float numberIn)
{
    currentLine=numberIn;
    return 1;
}

double AxisGroup::getMixDurationLeft()
{
    double tmpMaxTime=-1.0;
    for(int i=0;i<axisArry.size();i++)
    {
        if(tmpMaxTime<axisArry[i]->getMixDurationLeft())
        {
            tmpMaxTime=axisArry[i]->getMixDurationLeft();
        }
    }
    return tmpMaxTime;
}

int AxisGroup::setRotateEqradius(double rotate_eqradiusIn)
{
    if(coordinateAxisArry.size()>0)
    {
        coordinateAxisArry[0]->setRotateEqradius(rotate_eqradiusIn);
    }

    return 1;
}

int AxisGroup::ceaseAxisGroup()
{
    for(int i=0;i<axisArry.size();i++)
    {
          axisArry[i]->ceaseAxisObject();
    }
    return 1;
}

double AxisGroup::getAxisGroupStopTime()
{
    double tmpMaxTime=0.0;
    for(int i=0;i<axisArry.size();i++)
    {
        if(tmpMaxTime<axisArry[i]->getStopTime())
        {
            tmpMaxTime=axisArry[i]->getStopTime();
        }
    }
    return tmpMaxTime;

}

double AxisGroup::getAxisGroupVelLimitRatio()
{
    double tmpMaxTime=0.0;
    for(int i=0;i<axisArry.size();i++)
    {
        if(tmpMaxTime<axisArry[i]->getVelLimitRatio())
        {
            tmpMaxTime=axisArry[i]->getVelLimitRatio();
        }
    }
    return tmpMaxTime;
}

int AxisGroup::setAxisProperty(int axisId, AxisProperty_hmi axisPropertyIn)
{
    if(axisId<0||axisId>=axisArry.size())
    {
        return -1;
    }
    axisGroupDecouple->setAxisRatedTorque(axisId,axisPropertyIn.motorRatedTorque);
    axisArry[axisId]->setAxisProperty(axisPropertyIn);
    return 1;
}

int AxisGroup::setAxisPositionLimit(int axisId, double minPositionIn, double maxPositionIn,
                                    double approachDistanceIn)
{
    if(axisId<0||axisId>=axisArry.size())
    {
        return -1;
    }
    axisArry[axisId]->setAxisPositionLimit(minPositionIn,maxPositionIn,approachDistanceIn);
    return 1;
}

int AxisGroup::setVelocityStyle(E_VELOCITY_TYPE velocityStypeIn)
{
    if(coordinateAxisArry.size()>0)
    {
        coordinateAxisArry[0]->setVelocityStyle(velocityStypeIn);
    }
//    for(int i=0;i<axisArry.size();i++)
//    {
//        axisArry[i]->setVelocityStyle(velocityStypeIn);
//    }
    return 1;

}

int AxisGroup::setAccelerationRatio(double ratioIn)
{
    if(coordinateAxisArry.size()>0)
    {
        coordinateAxisArry[0]->setAccelerationRatio(ratioIn);
    }
    for(int i=0;i<totalAxisNumber;i++)
    {
        axisArry[i]->setAccelerationRatio(ratioIn);
    }
    return 1;


}

int AxisGroup::setAccelerationRatio_RobotArmJoint(double ratioIn)
{
    for(int i=0;i<totalAxisNumber;i++)
    {
        axisArry[i]->setAccelerationRatio(ratioIn);
    }
    return 1;
}

int AxisGroup::setAccelerationRatio_RobotArmLine(double ratioIn)
{
    if(coordinateAxisArry.size()>0)
    {
        coordinateAxisArry[0]->setAccelerationRatio(ratioIn);
    }
    return 1;
}

int AxisGroup::setRobotArmLineMaxAcceleration(double accelerationIn)
{
    if(coordinateAxisArry.size()>0)
    {
        return coordinateAxisArry[0]->setAcceleration(accelerationIn);
    }
    return -1;

}

int AxisGroup::setRobotArmLineMaxVelocity(double velIn)
{
    if(coordinateAxisArry.size()>0)
    {
        return coordinateAxisArry[0]->setMaxVelocity(velIn);
    }
    return -1;
}

int AxisGroup::setRobotArmJointMaxAcceleration(int axisIdIn, double accelerationIn)
{
    if(axisIdIn>=0 && axisIdIn<axisArry.size())
    {
        return axisArry[axisIdIn]->setMaxAcceleration(accelerationIn);
    }
    return -1;
}

int AxisGroup::setRobotArmJointMaxVelocity(int axisIdIn, double velIn)
{
    if(axisIdIn>=0 && axisIdIn<axisArry.size())
    {
        return axisArry[axisIdIn]->setMaxVel(velIn);
    }
    return -1;
}

E_ZERO_TASK_MODE AxisGroup::getZeroTaskMode()
{
    return zeroManagement->getZeroTaskMode();
}

int AxisGroup::getMotorEncoderPulsePerRound(QVector<double> &pulseOut)
{
    return axisGroupDecouple->getMotorEncoderPulsePerRound(pulseOut);
}

int AxisGroup::setServoOnStatus(const QVector<bool> &servoOnStatus)
{
    for(int i=0;i<servoOnStatus.size();i++)
    {
        setServoOnStatus(i,servoOnStatus[i]);
    }
    return 1;
}

int AxisGroup::setServoOnStatus(int axisId,bool isOn)
{
    if(axisId<0||axisId>=axisArry.size())
    {
        return -1;
    }
    axisArry[axisId]->setServoOnStatus(isOn);
    return 1;
}

int AxisGroup::axisFeedbackCheck()
{
    for(int i=0;i<totalAxisNumber;i++)
    {
        if(1!=axisArry[i]->axisFeedbackCheck())
        {
            return -1;
        }
    }
    return 1;
}

int AxisGroup::isPositionCommandApprochLimit()
{
    int tmpKey;
    for(int i=0;i<totalAxisNumber;i++)
    {
        tmpKey=axisArry[i]->isPositionCommandApprochLimit();
        if(0 !=tmpKey)
        {
            return tmpKey;
        }
    }
    return 0;
}

int AxisGroup::setAgvAxisVel(double velIn, int agvAxisId)
{
    int tmpAxisid=0;
    for(int i=0;i<totalAxisNumber;i++)
    {
        if(E_AXIS_LOGIC_TYPE_AGV==axisGroupAttribute[i].axisLogicalType )
        {
            if(agvAxisId==tmpAxisid)
            {
//                return axisArry[i]->moveOneStepByV_filter(velIn);
                return axisArry[i]->moveOneStepByV(velIn);
            }
            else
            {
                tmpAxisid++;
            }

        }

    }


    return -1;


}

int AxisGroup::setAgvSteeringPosition(double positionIn, int agvAxisId)
{
    int tmpAxisid=0;
    for(int i=0;i<totalAxisNumber;i++)
    {
        if(E_AXIS_LOGIC_TYPE_AGVSTEERING==axisGroupAttribute[i].axisLogicalType )
        {
            if(agvAxisId==tmpAxisid)
            {
                return axisArry[i]->moveOneStepByPosition(positionIn);
            }
            else
            {
                tmpAxisid++;
            }

        }

    }


    return -1;
}

int AxisGroup::setAgvSteeringPosition(QVector<double> positionIn)
{
    for(int i=0;i<positionIn.size();i++)
    {
        setAgvSteeringPosition(positionIn[i],i);
    }

    return 1;
}

int AxisGroup::getAgvSteeringPositionFeedback(QVector<double> &positionOut)
{
    positionOut.clear();
    double tmpPosition;
    for(int i=0;i<totalAxisNumber;i++)
    {
        if(E_AXIS_LOGIC_TYPE_AGVSTEERING==axisGroupAttribute[i].axisLogicalType )
        {
            tmpPosition=axisArry[i]->positionFeedback;
            positionOut.append(tmpPosition);
        }

    }


    return 1;
}

int AxisGroup::calibrateJointByAiEncoder(int axisId, double aiValue)
{
    return zeroManagement->calibrateJointByAiEncoder(axisId,aiValue);
}

int AxisGroup::setAgvAxisVel(QVector<double> velIn)
{

    for(int i=0;i<velIn.size();i++)
    {
        setAgvAxisVel(velIn[i],i);
    }

    return 1;
}

bool AxisGroup::isAbsoluteMotor(int axisId)
{
    return zeroManagement->isAbsoulteMotor(axisId);
}

int AxisGroup::startGoZero(int axisId)
{
    return zeroManagement->startGoZero(axisId);
}

int AxisGroup::startGoZero()
{
    return zeroManagement->startGoZero();
}

int AxisGroup::haltGoZero()
{
    return zeroManagement->haltGoZero();
}

int AxisGroup::getGoZeroAxis(int &axisId)
{
    return zeroManagement->getGoZeroAxis(axisId);
}

int AxisGroup::changeToNextGoZeroAxis()
{
    return zeroManagement->changeToNextGoZeroAxis();
}

int AxisGroup::getZeroMovingType(EM_ZERO_MOVINT_TYPE &zeroMovingType)
{
    return zeroManagement->getZeroMovingType(zeroMovingType);
}

int AxisGroup::zeroManagementLoopRealTime()
{
    return zeroManagement->loopRealTime();
}

int AxisGroup::zeroManagementLoopCommonTime()
{
    return zeroManagement->loopCommonTime();
}

int AxisGroup::getZeroReturnParameter(QVector<ZeroReturnParameter> &parameterOut)
{
    zeroManagement->getZeroReturnParameter(parameterOut);
    return 1;
}

int AxisGroup::getDebugInfo(AxisGroupDebugInfo &infoOut)
{
    infoOut.deltaTime=deltaTime;
    infoOut.stage=debugInfo.stage;
    return 1;
}

int AxisGroup::restoreControlMode(int axisId)
{
    if(axisId<0||axisId>=axisArry.size())
    {
        return -1;
    }
    axisArry[axisId]->restoreControlMode();
    return 1;
}

int AxisGroup::restoreControlMode()
{
    for(int i=0;i<axisArry.size();i++)
    {
        axisArry[i]->restoreControlMode();
    }
    return 1;
}

int AxisGroup::getMotorControlMode(QVector<E_MOTOR_CONTROL_MODE> &controlModeOut)
{
    controlModeOut.clear();
    E_MOTOR_CONTROL_MODE tmpMode;
    for(int i=0;i<axisArry.size();i++)
    {
        axisArry[i]->getMotorControlMode(tmpMode);
        controlModeOut.append(tmpMode);
    }

    return 1;
}

int AxisGroup::getMotorControlMode(int axisId,E_MOTOR_CONTROL_MODE &modeOut)
{
    if(axisId<0||axisId>=axisArry.size())
    {
        return -1;
    }
    axisArry[axisId]->getMotorControlMode(modeOut);
    return 1;
}


int AxisGroup::setMotorControlMode(int axisId, E_MOTOR_CONTROL_MODE modeIn)
{
    if(axisId<0||axisId>=axisArry.size())
    {
        return -1;
    }
    axisArry[axisId]->setMotorControlMode(modeIn);
    return 1;
}

int AxisGroup::setMotorControlMode(E_MOTOR_CONTROL_MODE modeIn)
{
    for(int i=0;i<axisArry.size();i++)
    {
        axisArry[i]->setMotorControlMode(modeIn);
    }
    return 1;
}

int AxisGroup:: initialTimeAxis()
{
    //时间轴初始化
    JointAttribute attribute;
    //trap用
//    attribute.maxAccelation=6;
//    //attribute.maxVelocity=1.7;
//    attribute.maxVelocity=3;
    //spline用
//        attribute.maxAccelation=6;
//        attribute.maxVelocity=2;

    //trap用
    attribute.maxAccelation=30;
    //attribute.maxVelocity=1.7;
//    attribute.maxVelocity=20;
    attribute.maxVelocity=1000;

    attribute.maxTorque=10000;
    attribute.maxPosition=1.1;
    attribute.minPosition=-0.1;
    attribute.coupleFlag=0;
    attribute.coupleJoint=0;
    attribute.coupleReducer=0;
    attribute.jointType=0;
    attribute.pulsePerRound=100;
    attribute.externAxisFlag=0;
    attribute.gearRatio=1;
    attribute.jointName="timeAxis";
    attribute.mobile=false;
    attribute.processFollowError=2;
    attribute.screwPitch=1;
    attribute.targetFollowError=1;
    attribute.axisLogicalType=E_AXIS_LOGIC_TYPE_VIRTUAL_STAND_ALONE;

    if(0==totalAxisNumber)
    {
        timeAxis.resize(1);
        timeAxis[0]=new AxisObject(robotId,attribute,motionMessage);
        timeAxis[0]->axisIdByRobot=20000;
    }
    else if(0<totalAxisNumber)
    {
        timeAxis.resize(totalAxisNumber);
        for(int i=0 ;i<totalAxisNumber;i++)
        {
            timeAxis[i]=new AxisObject(robotId,attribute,motionMessage);
            timeAxis[i]->axisIdByRobot=20000+i;
        }
    }
    return 1;
}

bool AxisGroup::isInFineMoving()
{

//    int validAxisSize=axisIdVector.size();
//    for(int i=0;i<validAxisSize;i++)
//    {
//        if(false==axisArry[axisIdVector[i]]->isTheLastTrajCommandMoving())
//        {
//            return false;
//        }
//    }
//    for(int i=0;i<coordinateAxisNumber;i++)
//    {
//        if(false==coordinateAxisArry[axisIdVector[i]]->isTheLastTrajCommandMoving())
//        {
//            return false;
//        }
//    }
//    return true;

    return coordinateAxisArry[0]->isInFineMoving();
}

int AxisGroup::createAxisObject(int axisId, JointAttribute axisAttribute)
{

    AxisObject* tmpAxis=new AxisObject(robotId,axisAttribute,motionMessage);
    tmpAxis->axisIdByRobot=axisId;
    axisArry.append(tmpAxis);

    return 1;

}

int AxisGroup::createCoordinateAxisObject(int axisId,double deltaTimeIn, JointAttribute axisAttribute
                                          ,double rotate_eqradius)
{

    //1,5.delta太快
    CoordinateTrajectory* tmpAxis=new CoordinateTrajectory(robotId,deltaTimeIn,axisAttribute,motionMessage,
                          rotate_eqradius,5);
    tmpAxis->axisIdByRobot=axisId;
    coordinateAxisArry.append(tmpAxis);

    return 1;

}

double AxisGroup::getLineRemainingTime()
{
    return currentLineRemainingTime;
}

//int AxisGroup::cease()
//{
//    setTimeAxisScale(0.0, ceaseTimeDuration);
//    while(0.1e-5 < timeAxis->positionCommand  );
//    return 1;
//}

//int AxisGroup::stop()
//{
//    setTimeAxisScale(0.0, ceaseTimeDuration);
//    while(0.1e-5 < timeAxis->positionCommand  );
//    reset();
//    return 1;
//}


double AxisGroup::getTimeAxisScale()
{
    return timeAxis[0]->positionCommand;
}

double AxisGroup::getTimeAxisScale(int timeAxisIndex)
{
    return timeAxis[timeAxisIndex]->positionCommand;
}

int AxisGroup::clearTraj_byAxisId(int axisIndex,bool isSyncFeedbackPos)
{

    axisArry[axisIndex]->clearTraj();

    if(isSyncFeedbackPos)
    {
         upDateAllFeedbackAndOffset();
    }


    return 1;
}

int AxisGroup::resetCoordinateTraj(int axisIndex)
{

    coordinateAxisArry[axisIndex]->clearTraj();

    upDateAllFeedbackAndOffset();

    return 1;
}

int AxisGroup::clearTraj(bool isSyncFeedbackPos)
{

    groupTrajQueue.clear();
    for(int i=0;i<totalAxisNumber;i++)
    {
        axisArry[i]->clearTraj();
    }
    for(int i=0;i<coordinateAxisNumber;i++)
    {
        coordinateAxisArry[i]->clearTraj();
    }
    if(isSyncFeedbackPos)
    {
        upDateAllFeedbackAndOffset();
    }


    return 1;
}

int AxisGroup::clearFrontTraj()
{
    exceedTimeCount=0;
    if(0==groupTrajQueue.size())
    {
        return 0;
    }


    if(groupTrajQueue.front().isJointTraj)
    {
        groupTrajQueue.dequeue();
        return clearFrontTraj_joint();
    }
    else
    {
        groupTrajQueue.dequeue();
        return clearFrontTraj_decart();
    }
    return 1;
}

int AxisGroup::clearFrontTraj_joint()
{

    for(int i=0;i<totalAxisNumber;i++)
    {
        axisArry[i]->clearTraj_front();
    }
    return 1;

}

int AxisGroup::clearFrontTraj_decart()
{

    for(int i=0;i<coordinateAxisNumber;i++)
    {
        coordinateAxisArry[i]->clearTraj_front();
    }
    return 1;
}

int AxisGroup::addTrajInfo(bool isJointTraj,double timeDuration_math,int cnt,int lineId)
{
    GroupTrajeAttribute tmpTrajInfo;
    tmpTrajInfo.isJointTraj=isJointTraj;
    tmpTrajInfo.timeDuration_math=timeDuration_math;
    tmpTrajInfo.cnt=cnt;
    tmpTrajInfo.lineId=lineId;
    tmpTrajInfo.timeDuration_norminal=-1;
    if(0>=cnt)
    {
        tmpTrajInfo.endCutoffTime=0;
    }
    else
    {
        tmpTrajInfo.endCutoffTime=-1;
    }
    tmpTrajInfo.startCutoffTime=0;
    groupTrajQueue.enqueue(tmpTrajInfo);

    if(isJointTraj)
    {
        setPositionFrameCommandPlaned_byJointCommandPlaned();
    }
    else
    {
        setJointCommandPlaned_byPositionFrameCommandPlaned();
    }
    return 1;
}
int AxisGroup::resetAll()
{

    isInDecartTraj=false;
    exceedTimeCount=0;
    isInCntPlan=false;
    groupTrajQueue.clear();
    //upDateAllFeedbackAndOffset(); ethercat线程有一直处理，重复了。
    for(int i=0;i<totalAxisNumber;i++)
    {
        axisArry[i]->resetAll();
    }
    for(int i=0;i<coordinateAxisNumber;i++)
    {
        coordinateAxisArry[i]->resetAll();
    }
    for(int i=0;i<totalAxisNumber;i++)
    {
        timeAxis[i]->resetAll();
    }
    return 1;
}

//int AxisGroup::syncCommandAndFeedback()
//{
//    for(int i=0;i<totalAxisNumber;i++)
//    {
//        axisArry[i]->resetAll();
//    }
//    return 1;
//}
int AxisGroup::setAllTimeAxisScale(double ratio, double timeDuration)
{
    for(int i=0;i<totalAxisNumber;i++)
    {
        timeAxis[i]->clearTraj();//保证立刻覆盖前面的旧轨迹
        timeAxis[i]->splineZeroMoveByTime(ratio,timeDuration);
    }
    return 1;
}

int AxisGroup::setNewTimeAxisVelLimitRatio_coordAxis(double scaleIn)
{
    for(int i=0;i<coordinateAxisNumber;i++)
    {
        coordinateAxisArry[i]->setNewTimeAxisVelLimitRatio(scaleIn);
    }
    return 1;
}

int AxisGroup::restoreTimeAxisVelLimitRatio_coordAxis(double scaleIn)
{
    for(int i=0;i<coordinateAxisNumber;i++)
    {
        coordinateAxisArry[i]->restoreTimeAxisVelLimitRatio(scaleIn);
    }
    return 1;
}

int AxisGroup::resetTimeAxisVelLimitRatio_coordAxis()
{
    for(int i=0;i<coordinateAxisNumber;i++)
    {
        coordinateAxisArry[i]->resetTimeAxisVelLimitRatio();
    }
    return 1;
}

int AxisGroup::setTimeAxisScale(double ratio, double timeDuration, int timeAxisIndex)
{
    timeAxis[timeAxisIndex]->clearTraj();//保证立刻覆盖前面的旧轨迹
    timeAxis[timeAxisIndex]->splineZeroMoveByTime(ratio,timeDuration);
    return 1;
}

int AxisGroup::setTimeAxisScaleByTrap(double ratio, int timeAxisIndex, int acceleration)
{
    timeAxis[timeAxisIndex]->clearTraj();//保证立刻覆盖前面的旧轨迹
    timeAxis[timeAxisIndex]->trapMove(ratio,0.3,acceleration);
    return 1;
}

int AxisGroup::setTimeAxisScaleBySpline(double ratio,int timeAxisIndex)
{
    timeAxis[timeAxisIndex]->clearTraj();//保证立刻覆盖前面的旧轨迹
    timeAxis[timeAxisIndex]->splineZeroMove(ratio,0.8);
    return 1;
}

int AxisGroup::setDeltaTime(double time)
{
    deltaTime=time;
    for(int i=0;i<totalAxisNumber;i++)
    {
        axisArry[i]->setDeltaTime(time);
    }
    for(int i=0;i<coordinateAxisNumber;i++)
    {
        coordinateAxisArry[i]->setDeltaTime(time);
    }
    for(int i=0;i<totalAxisNumber;i++)
    {
        timeAxis[i]->setDeltaTime(time);
    }
     return 1;
}

int AxisGroup::timeAxisMoveOneStep(QVector<double>& timeAxisScaleOut)
{
    timeAxisScaleOut.resize(totalAxisNumber);
    for(int i=0;i<totalAxisNumber;i++)
    {
        timeAxis[i]->moveOneStep();
        timeAxisScaleOut[i]=timeAxis[i]->positionCommand;
    }
    //更新所有轴timeScale;
     for(int i=0;i<totalAxisNumber;i++)
     {
         axisArry[i]->setTimeAxisScale(timeAxis[i]->positionCommand);
     }
     for(int i=0;i<coordinateAxisNumber;i++)
     {
         coordinateAxisArry[i]->setTimeAxisScale(timeAxis[0]->positionCommand);
     }

    return 1;
}

int AxisGroup::moveOneStep()
{
    debugInfo.stage=E_AXISGROUP_STAGE_MOVE_ONE_STEP_IN;
    int isMoved=-1;

     int moveOneResult;
     //坐标轴轴运行一步
     debugInfo.stage=E_AXISGROUP_STAGE_COORDINATE_AXIS_ONE_STEP;
     for(int i=0;i<coordinateAxisNumber;i++)
     {

         moveOneResult=coordinateAxisArry[i]->moveOneStep_mixedPosition();

         if(1==moveOneResult)
         {
            isMoved=1;
//            getCoordinateAxisPositionCommand(finishedPositionFrameCommand);
            //坐标轴数据转移到关节轴数据.TODO_waveMove摆弧
            if(-1==transeferCoordinateAxisData2JointAxisData())
            {
                debugInfo.stage=E_AXISGROUP_STAGE_MOVE_ONE_STEP_FINISH_ERROR;
                return -2;
            }
//             moveOneStepByPosition(coordinateAxisArry[i]->positionFrameCommand.p[0],0);
//             moveOneStepByPosition(coordinateAxisArry[i]->positionFrameCommand.p[1],1);
//             moveOneStepByPosition(coordinateAxisArry[i]->positionFrameCommand.p[2],2);
//             double alfa, beta, gamma;
//             coordinateAxisArry[i]->positionFrameCommand.M.GetEulerZYX(alfa,beta,gamma);
//             moveOneStepByPosition(alfa,3);
//             moveOneStepByPosition(beta,4);
//             moveOneStepByPosition(gamma,5);
         }
         else if(-2==moveOneResult)
         {
             //addMsg(ENUM_MSG_ERROR,"MotionServer","AxisGroup",5800,robotId, i);
             debugInfo.stage=E_AXISGROUP_STAGE_MOVE_ONE_STEP_FINISH_ERROR;
             return -2;
         }

     }



     //关节轴运行一步
     debugInfo.stage=E_AXISGROUP_STAGE_JOINT_AXIS_ONE_STEP;
     for(int i=0;i<totalAxisNumber;i++)
     {

         moveOneResult=axisArry[i]->moveOneStep();

         if(1==moveOneResult)
         {
            isMoved=1;
         }
         else if(-2==moveOneResult)
         {
             //addMsg(ENUM_MSG_ERROR,"MotionServer","AxisGroup",5800,robotId, i);
             debugInfo.stage=E_AXISGROUP_STAGE_MOVE_ONE_STEP_FINISH_ERROR;
             return -2;
         }

     }


     debugInfo.stage=E_AXISGROUP_STAGE_MOVE_ONE_STEP_FINISH_NORMAL;
     return isMoved;
}

int AxisGroup::moveOneStep_cnt()
{
    if(0==groupTrajQueue.size())
    {
//        qDebug()<<"0==groupTrajQueue.size(),,,AxisGroup::moveOneStep_cnt";
        return -1;
    }
    if(0==updateCntCutoffTime())
    {
        qDebug()<<"**********************0==updateCntCutoffTime()";
        for(int i=0;i<groupTrajQueue.size();i++)
        {
            qDebug()<<"cnt"<<groupTrajQueue[i].cnt;
        }
        return 0;//平滑段未准备完
    }
    int tmpReturnKey=0;
    double accumulatedTime,tmpDuration;
    if(groupTrajQueue.front().isJointTraj)
    {
        accumulatedTime=getAccumulatedTime_joint();
    }
    else
    {
        accumulatedTime=getAccumulatedTime_decart();
    }
    tmpDuration=groupTrajQueue.front().timeDuration_math;


    std::vector<bool> isMoveTraj1,isMoveTraj2;
    isMoveTraj1.resize(armControlAxisNumber);
    isMoveTraj2.resize(armControlAxisNumber);
    for(int i=0;i<armControlAxisNumber;i++)
    {
        isMoveTraj1[i]=false;
        isMoveTraj2[i]=false;
    }
    int moveOneResult;

    //参考位置
    vector<double> positonInitialList;
    positonInitialList.resize(armControlAxisNumber);
    for(int i=0;i<armControlAxisNumber;i++)
    {
       positonInitialList[i]=axisArry[i]->positionCommand;
    }
    string configString;
    std::vector<double> positionValue1,positionValue2;
    positionValue1.resize(armControlAxisNumber);
    positionValue2.resize(armControlAxisNumber);

    bool tmpIsInDecartTraj=false;

    //第一轨迹
    currentLineRemainingTime=tmpDuration-accumulatedTime;
    if(accumulatedTime <=tmpDuration )
    {
        if(groupTrajQueue.front().isJointTraj)
        {
            //关节轴运行一步
            debugInfo.stage=E_AXISGROUP_STAGE_JOINT_AXIS_ONE_STEP;
            for(int i=0;i<armControlAxisNumber;i++)
            {

                moveOneResult=axisArry[i]->moveOneStepByTrajIndex(0);

                if(1==moveOneResult)
                {
                   positionValue1[i]=axisArry[i]->getPositionCommand_cnt();
                   isMoveTraj1[i]=true;
                }
                else if(moveOneResult<0)
                {
                    errorStatusFlag=1;
                    //addMsg(ENUM_MSG_ERROR,"MotionServer","AxisGroup",5800,robotId, i);
                    debugInfo.stage=E_AXISGROUP_STAGE_MOVE_ONE_STEP_FINISH_ERROR;
                    qDebug()<<"1  moveOneResult<0";
                    return moveOneResult;
                }

            }

        }
        else
        {
            debugInfo.stage=E_AXISGROUP_STAGE_COORDINATE_AXIS_ONE_STEP;
            //坐标轴轴运行一步
            moveOneResult=coordinateAxisArry[0]->moveOneStepByTrajIndex(0);
            if(1==moveOneResult)
            {
                tmpIsInDecartTraj=true;
                for(int i=0;i<armControlAxisNumber;i++)
                {
                    isMoveTraj1[i]=true;
                }
               //坐标轴数据转移到关节轴数据.
               PointPro stepPoint;
               bool singuratyStatus;
               if(1==coordinateManager->userFrameToPointPro_JOINT(coordinateAxisArry[0]->positionFrameCommand_cnt,
                                       stepPoint,singuratyStatus,configString,positonInitialList ))
               {

                    positionValue1=stepPoint.positionValue;
                    if(singuratyStatus)
                    {
                        errorStatusFlag=2;
                    }
               }
               else
               {
                   errorStatusFlag=1;
                   qDebug()<<"transeferCoordinateAxisData2JointAxisData222 error,1!=coordinateManager->frameToPointPro_JOINT ";
                   return -1;
               }



            }
            else if(moveOneResult<0)
            {
                errorStatusFlag=1;
                //addMsg(ENUM_MSG_ERROR,"MotionServer","AxisGroup",5800,robotId, i);
                debugInfo.stage=E_AXISGROUP_STAGE_MOVE_ONE_STEP_FINISH_ERROR;
                qDebug()<<"2  moveOneResult<0";
                return moveOneResult;
            }
        }

    }
    else
    {

    }


    //第二轨迹
    double positionMixRatio;
    if(groupTrajQueue.size()>1)
    {
        GroupTrajeAttribute tmpTrajAttri=groupTrajQueue.front();
        if(currentLineRemainingTime<=tmpTrajAttri.endCutoffTime &&
                                            tmpTrajAttri.cnt>0)
        {
            if(currentLineRemainingTime>tmpTrajAttri.endCutoffTime/2.0)
            {
                currentLine=groupTrajQueue.front().lineId+0.5;

            }
            else
            {
                currentLine=groupTrajQueue[1].lineId+0.5;

            }
            //qDebug()<<"mixed----accumulatedTime"<<accumulatedTime;

            if(groupTrajQueue[1].isJointTraj)
            {
                int tmpIndex=0;
                if(groupTrajQueue.front().isJointTraj)
                {
                    tmpIndex=1;
                }
                else
                {
                    tmpIndex=0;
                }
                //关节轴运行一步
                debugInfo.stage=E_AXISGROUP_STAGE_JOINT_AXIS_ONE_STEP;
                for(int i=0;i<armControlAxisNumber;i++)
                {

                    moveOneResult=axisArry[i]->moveOneStepByTrajIndex(tmpIndex);

                    if(1==moveOneResult)
                    {
                        positionValue2[i]=axisArry[i]->getPositionCommand_cnt();
                        isMoveTraj2[i]=true;
                    }
                    else if(moveOneResult<0)
                    {
                        errorStatusFlag=1;
                        //addMsg(ENUM_MSG_ERROR,"MotionServer","AxisGroup",5800,robotId, i);
                        debugInfo.stage=E_AXISGROUP_STAGE_MOVE_ONE_STEP_FINISH_ERROR;
                        qDebug()<<"3  moveOneResult<0";
                        return moveOneResult;
                    }

                }

            }
            else
            {
                debugInfo.stage=(E_AXISGROUP_STAGE)6;
                int tmpIndex=0;
                if(groupTrajQueue.front().isJointTraj)
                {
                    tmpIndex=0;
                }
                else
                {
                    tmpIndex=1;
                }
                //坐标轴轴运行一步
                moveOneResult=coordinateAxisArry[0]->moveOneStepByTrajIndex(tmpIndex);
                if(1==moveOneResult)
                {
                    tmpIsInDecartTraj=true;
                    for(int i=0;i<armControlAxisNumber;i++)
                    {
                        isMoveTraj2[i]=true;
                    }
                   //坐标轴数据转移到关节轴数据.
                   PointPro stepPoint;
                   bool singuratyStatus;
                   if(1==coordinateManager->userFrameToPointPro_JOINT(coordinateAxisArry[0]->positionFrameCommand_cnt,
                                           stepPoint,singuratyStatus,configString,positonInitialList ))
                   {
                        positionValue2=stepPoint.positionValue;
                        if(singuratyStatus)
                        {
                            errorStatusFlag=2;
                        }
                   }
                   else
                   {
                       errorStatusFlag=1;
                       qDebug()<<"transeferCoordinateAxisData2JointAxisData222 error,1!=coordinateManager->frameToPointPro_JOINT ";
                       return -1;
                   }

                }
                else if(moveOneResult<0)
                {
                    errorStatusFlag=1;
                    //addMsg(ENUM_MSG_ERROR,"MotionServer","AxisGroup",5800,robotId, i);
                    debugInfo.stage=E_AXISGROUP_STAGE_MOVE_ONE_STEP_FINISH_ERROR;
                    qDebug()<<"4  moveOneResult<0";
                    return moveOneResult;
                }
            }
            positionMixRatio=1.0-currentLineRemainingTime/tmpTrajAttri.endCutoffTime;
//                isInPositionMixsing=true;

        }


    }
    else
    {
        currentLine=groupTrajQueue.front().lineId+0.5;
    }

    isInDecartTraj=tmpIsInDecartTraj;
    //混合轨迹位置
    bool tmpMovedFlag=false;
    for(int j=0;j<armControlAxisNumber;j++)
    {
        int tmpAxisResult=0;
        if(true==isMoveTraj1[j] && true==isMoveTraj2[j])
        {
            tmpReturnKey=1;
            tmpAxisResult=moveOneStepByPosition_withMixer(positionValue1[j],positionValue2[j]
                                            ,positionMixRatio,j);
            tmpMovedFlag=true;
        }
        else if(true==isMoveTraj1[j])
        {
            tmpReturnKey=1;
            tmpAxisResult=moveOneStepByPosition(positionValue1[j],j);
            tmpMovedFlag=true;
        }
        else if(true==isMoveTraj2[j])
        {
            tmpReturnKey=1;
            tmpAxisResult=moveOneStepByPosition(positionValue2[j],j);
            tmpMovedFlag=true;
        }
        if(tmpAxisResult<0)
        {
            errorStatusFlag=1;
            qDebug()<<"tmpAxisResult<0 errorStatusFlag=1";
        }

    }
    // 根据混合后的关节位置，反求世界位置。
    setPositionFrameCommand_byJointCommand();

    if(false==tmpMovedFlag)
    {
//        qDebug()<<"false==tmpMovedFlag";
    }


    if(accumulatedTime >= tmpDuration)
    {
        qDebug()<<"axisGroup clearTraj_front----accumulatedTime"<<accumulatedTime
               <<"tmpDuration"<<tmpDuration;
        if(1==groupTrajQueue.size())
        {
            if(isTargetArrived())
            {
                currentLine=groupTrajQueue.front().lineId+0.9;
                //清除轨迹。
                clearFrontTraj();
            }

        }
        else
        {
            //清除轨迹。
            clearFrontTraj();
        }



    }


    return tmpReturnKey;
}

int AxisGroup::updateCntCutoffTime()
{
    //整段一起计算。
    if(groupTrajQueue.back().cnt>0)
    {
        return 0;
    }
    isInCntPlan=false;
    double tmpCompareTime1,tmpCompareTime2;
    double tmpMinTime;
    for(int i=0;i<groupTrajQueue.size()-1;i++)
    {
        if(groupTrajQueue[i].endCutoffTime<0 )
        {

            if(false==isInCntPlan)//第一个
            {
                tmpCompareTime1=groupTrajQueue[i].timeDuration_math;
                if(i+1==groupTrajQueue.size()-1)//倒数第二个
                {
                    tmpCompareTime2=groupTrajQueue[i+1].timeDuration_math;
                }
                else
                {
                    tmpCompareTime2=groupTrajQueue[i+1].timeDuration_math/2.0;
                }
                if(tmpCompareTime1<tmpCompareTime2)
                {
                    tmpMinTime=tmpCompareTime1;
                }
                else
                {
                    tmpMinTime=tmpCompareTime2;
                }
                isInCntPlan=true;

            }
            else//其余
            {



                tmpCompareTime1=groupTrajQueue[i].timeDuration_math/2.0;
                if(i+1==groupTrajQueue.size()-1)
                {
                    tmpCompareTime2=groupTrajQueue[i+1].timeDuration_math;
                }
                else
                {
                    tmpCompareTime2=groupTrajQueue[i+1].timeDuration_math/2.0;
                }
                if(tmpCompareTime1<tmpCompareTime2)
                {
                    tmpMinTime=tmpCompareTime1;
                }
                else
                {
                    tmpMinTime=tmpCompareTime2;
                }
            }
            groupTrajQueue[i].endCutoffTime=tmpMinTime*groupTrajQueue[i].cnt/100.0;
        }
    }
    isInCntPlan=false;
    return 1;

}

int AxisGroup::isTargetArrived()
{
    exceedTimeCount++;
    if(1==isTargetOffsetCheckOk())
    {//已经到达
        exceedTimeCount=0;
        return 1;

    }
    else if(exceedTimeCount*deltaTime>targetWaitTime)//超过targetWaitTimeS 报错
    {//超时未到达
        exceedTimeCount=0;
        addMsg(ENUM_MSG_ERROR,"MotionServer","TrajectoryServer",5802,robotId,targetWaitTime*1000);

    }
    return 0;
}

int AxisGroup::moveOneStep_byWorldCoordinate(const Frame &worldFrameIn, bool isOverLay, double durationLeft)
{
    PointPro stepPoint;
    coordinateAxisNumber=1;

    vector<double> positonInitialList;
    positonInitialList.resize(armControlAxisNumber);
    for(int i=0;i<armControlAxisNumber;i++)
    {
       positonInitialList[i]=axisArry[i]->positionCommand;
    }

    string configString;
    bool isSinguratyOut;
    if(1==coordinateManager->worldFrameToPointPro_JOINT(worldFrameIn,stepPoint,isSinguratyOut,
                                                        configString,positonInitialList))
    {
//            qDebug()<<"userFrameToPointPro_JOINT"<<stepPoint.positionValue[0]<<stepPoint.positionValue[1]
//                      <<stepPoint.positionValue[2]<<stepPoint.positionValue[3];
        for(int j=0;j<stepPoint.positionValue.size();j++)
        {
            if(isOverLay)
            {
                moveOneStepByPosition_prepareMix(stepPoint.positionValue[j],durationLeft,j);
            }
            else
            {
               moveOneStepByPosition(stepPoint.positionValue[j],j);
            }

//                qDebug()<<"moveOneStepByPosition(stepPoint.positionValue[j]="<<stepPoint.positionValue[j]<<"  j="<<j;
        }
    }
    else
    {
        qDebug()<<"error,moveOneStep_byWorldCoordinate"<< worldFrameIn.p[0]<< worldFrameIn.p[1]
               << worldFrameIn.p[2];
        return -1;
    }


    return 1;
}

int AxisGroup::moveOneStep_byUserCoordinateCommand(const PointPro &pointIn)
{
    PointPro stepPoint;
    coordinateAxisNumber=1;

    vector<double> positonInitialList;
    positonInitialList.resize(armControlAxisNumber);
    for(int i=0;i<armControlAxisNumber;i++)
    {
       positonInitialList[i]=axisArry[i]->positionCommand;
    }
//    string configString;
//    Frame positionFrameCommand;todo
//    if(1==coordinateManager->userFrameToPointPro_JOINT(positionFrameCommand
//                                                       stepPoint,configString,positonInitialList ))

    if(1==coordinateManager->pointCoordinateToJoint(pointIn,positonInitialList,stepPoint))
    {
//            qDebug()<<"userFrameToPointPro_JOINT"<<stepPoint.positionValue[0]<<stepPoint.positionValue[1]
//                      <<stepPoint.positionValue[2]<<stepPoint.positionValue[3];
        for(int j=0;j<stepPoint.positionValue.size();j++)
        {
            moveOneStepByPosition(stepPoint.positionValue[j],j);
//                qDebug()<<"moveOneStepByPosition(stepPoint.positionValue[j]="<<stepPoint.positionValue[j]<<"  j="<<j;
        }
    }
    else
    {
        qDebug()<<"error,moveOneStep_byCoordinateCommand"<< pointIn.positionValue[0]<< pointIn.positionValue[1]<< pointIn.positionValue[2]
                  << pointIn.positionValue[3]<< pointIn.positionValue[4]<< pointIn.positionValue[5];
        return -1;
    }


    return 1;
}

int AxisGroup::getCoordinateAxisPositionCommand(Frame &positionFrameCommandOut)
{
    if(0<coordinateAxisNumber)
    {
        positionFrameCommandOut=coordinateAxisArry[0]->positionFrameCommand;
        return 1;
    }

    return -1;
}

int AxisGroup::getFinishedAxisPositionCommand(Frame &positionFrameCommandOut)
{

}

int AxisGroup::transeferCoordinateAxisData2JointAxisData()
{

    PointPro stepPoint,stepPoint_mix;
    coordinateAxisNumber=1;
    for(int i=0;i<coordinateAxisNumber;i++)
    {
        //转换为关节角度
        vector<double> positonInitialList;
        positonInitialList.resize(armControlAxisNumber);
        for(int i=0;i<armControlAxisNumber;i++)
        {
           positonInitialList[i]=axisArry[i]->positionCommand;
        }
        string configString;
        bool isSinguratyOut;
        if(1==coordinateManager->userFrameToPointPro_JOINT(coordinateAxisArry[i]->positionFrameCommand,
                                              stepPoint,isSinguratyOut,configString,positonInitialList ))
        {

        }
        else
        {
            qDebug()<<"transeferCoordinateAxisData2JointAxisData error,1!=coordinateManager->frameToPointPro_JOINT ";
            return -1;
        }


        double tmpMixRatio;
        if(coordinateAxisArry[i]->getMixRatioOut(tmpMixRatio))//需要cnt融合
        {
            bool isSinguratyOut;
            if(1==coordinateManager->userFrameToPointPro_JOINT(coordinateAxisArry[i]->positionFrameCommand_mixed,
                                         stepPoint_mix,isSinguratyOut,configString,positonInitialList ))
            {

            }
            else
            {
                qDebug()<<"transeferCoordinateAxisData2JointAxisData error222,1!=coordinateManager->frameToPointPro_JOINT ";
                return -1;
            }

            for(int j=0;j<armControlAxisNumber;j++)
            {
                moveOneStepByPosition_withMixer(stepPoint.positionValue[j],stepPoint_mix.positionValue[j]
                                                ,tmpMixRatio,j);
            }
            // 根据混合后的关节位置，反求世界位置。
            setPositionFrameCommand_byJointCommand();
        }
        else//不需要cnt融合
        {
            for(int j=0;j<armControlAxisNumber;j++)
            {
                moveOneStepByPosition(stepPoint.positionValue[j],j);
            }
        }


    }

    return 1;
}


int AxisGroup::moveOneStepByPosition(double positionIn,int axisId)
{
    if(0>axisId || totalAxisNumber<=axisId)
    {
        return -1;
    }
    return axisArry[axisId]->moveOneStepByPosition(positionIn);
}

int AxisGroup::moveOneStepByPosition_plus(double positionPlus, int axisId)
{
    if(0>axisId || totalAxisNumber<=axisId)
    {
        return -1;
    }
    return axisArry[axisId]->moveOneStepByPosition_plus(positionPlus);
}

int AxisGroup::moveOneStepByPosition_prepareMix(double positionIn, double durationLeftIn, int axisId)
{
    if(0>axisId || totalAxisNumber<=axisId)
    {
        return -1;
    }
    return axisArry[axisId]->moveOneStepByPosition_prepareMix(positionIn,durationLeftIn);
}

int AxisGroup::moveOneStepByPosition_overlay(double positionIn, int axisId)
{
    if(0>axisId || totalAxisNumber<=axisId)
    {
        return -1;
    }
    return axisArry[axisId]->moveOneStepByPosition_overlay(positionIn);
}

int AxisGroup::moveOneStepByPosition_withMixer(double positionIn1,double positionIn2,
                                               double mixRatioIn, int axisId)
{
    if(0>axisId || totalAxisNumber<=axisId)
    {
        return -1;
    }
    return axisArry[axisId]->moveOneStepByPosition_withMixer(positionIn1,positionIn2,mixRatioIn);
}

int AxisGroup::moveOneStepByPosition_vel(double velIn, int axisId)
{
    if(0>axisId || totalAxisNumber<=axisId)
    {
        return -1;
    }
    return axisArry[axisId]->moveOneStepByPosition_vel(velIn);
}

int AxisGroup::moveOneStepByPV(double positionIn, double velIn ,int axisId)
{
    if(0>axisId || totalAxisNumber<=axisId)
    {
        return -1;
    }
    return axisArry[axisId]->moveOneStepByPV(positionIn,velIn);
}

int AxisGroup::moveOneStepByV(double velIn,int axisId)
{
    if(0>axisId || totalAxisNumber<=axisId)
    {
        qDebug()<<"failed, moveOneStepByV axisId"<<axisId;
        return -1;
    }
    return axisArry[axisId]->moveOneStepByV(velIn);
}

int AxisGroup::moveOneStepByV_filter(double velIn, int axisId)
{
    if(0>axisId || totalAxisNumber<=axisId)
    {
        return -1;
    }
    return axisArry[axisId]->moveOneStepByV_filter(velIn);
}

int AxisGroup::moveOneStepByPVAT(double positionIn, double velIn, double accIn, double torqueIn,int axisId)
{
    if(0>axisId || totalAxisNumber<=axisId)
    {
        return -1;
    }
    return axisArry[axisId]->moveOneStepByPVAT(positionIn,velIn,accIn,torqueIn);
}

int AxisGroup::moveOneStepByTorque(double torqueIn, int axisId)
{
    if(0>axisId || totalAxisNumber<=axisId)
    {
        return -1;
    }
    return axisArry[axisId]->moveOneStepByTorque(torqueIn);
}

 int AxisGroup::isAllVelProfileFinished(QVector<int> axisIdVector)
 {
     //判断是否有轴的velProfile被占用
     int validAxisSize=axisIdVector.size();
     for(int i=0;i<validAxisSize;i++)
     {
         if(0==axisArry[axisIdVector[i]]->isTrajComposeEmpty())
         {
             return 0;
         }
     }
     for(int i=0;i<coordinateAxisNumber;i++)
     {
         if(axisIdVector.size()>0)
         {
             if(axisIdVector[i]>=0 && axisIdVector[i]<coordinateAxisArry.size())
             {
                 if(0==coordinateAxisArry[axisIdVector[i]]->isVelProfileEmpty())
                 {
                     return 0;
                 }
             }
         }

     }
     return 1;
 }

 int AxisGroup::isAllCoordinateVelProfileFinished(QVector<int> axisIdVector)
 {
     //判断是否有轴的velProfile被占用
     int validAxisSize=axisIdVector.size();
     for(int i=0;i<validAxisSize;i++)
     {
         if(0==coordinateAxisArry[axisIdVector[i]]->isVelProfileEmpty())
         {
             return 0;
         }
     }
     return 1;
 }

 int AxisGroup::isAllVelProfileFinished()
 {
     //判断是否有轴的velProfile被占用
     for(int i=0;i<totalAxisNumber;i++)
     {
         if(0==axisArry[i]->isTrajComposeEmpty())
         {
             return 0;
         }
     }
     for(int i=0;i<coordinateAxisNumber;i++)
     {
         if(0==coordinateAxisArry[i]->isVelProfileEmpty())
         {
             return 0;
         }
     }
     return 1;
 }

 int AxisGroup::isTargetOffsetCheckOk()
 {
     //判断是否有轴的velProfile被占用
     for(int i=0;i<totalAxisNumber;i++)
     {
         if(-1==axisArry[i]->isTargetOffsetCheckOk())
         {
             return -1;
         }
     }
     return 1;
 }

 Frame AxisGroup::getPositionFrameCommand()
 {
     Frame tmpFrame;
     if(0>=coordinateAxisNumber)
     {
         qDebug()<<"getPositionFrameCommand error:0>=coordinateAxisNumber";
         return tmpFrame;
     }
     return coordinateAxisArry[0]->getPositionFrameCommand();
 }

int AxisGroup::setFrameFeedback(QVector<Frame> frameList)
{
    if(frameList.size()!=coordinateAxisNumber)
    {
        qDebug()<<"setFrameFeedback error:frameList.size()!=coordinateAxisNumber";
        return -1;
    }
    for(int i=0;i<coordinateAxisNumber;i++)
    {
        coordinateAxisArry[i]->setPositionFrameFeedback(frameList[i]);
    }
    return 1;
}

//int AxisGroup::setFrameCommand(QVector<Frame> frameList)
//{
//    if(frameList.size()!=coordinateAxisNumber)
//    {
//        qDebug()<<"setFrameFeedback error:frameList.size()!=coordinateAxisNumber";
//        return -1;
//    }
//    for(int i=0;i<coordinateAxisNumber;i++)
//    {
//        coordinateAxisArry[i]->setPositionFrameCommand(frameList[i]);
//    }
//    return 1;
//}

int AxisGroup::setFrameCommand_one(Frame frameIn)
{
    if(0>=coordinateAxisNumber)
    {
        qDebug()<<"setFrameFeedback error:0>=coordinateAxisNumber";
        return -1;
    }
    coordinateAxisArry[0]->setPositionFrameCommand(frameIn);
    return 1;
}

int AxisGroup::setPositionFrameCommand_byJointCommand()
{

    Frame tmpFrame;
    if(1!=calculateCurrentFrameCommandPoint_byJointCommand(tmpFrame))
    {
        qDebug()<<"error,,,,,,,,,,,,,,,,,setPositionFrameCommand_byJointCommand ";
        return -1;
    }
//    QVector<Frame> frameList;
//    frameList.append(tmpFrame);
    setFrameCommand_one(tmpFrame);
    return 1;
}

int AxisGroup::setPositionFrameCommandPlaned_byJointCommandPlaned()
{
    QVector<double> jointsPositionTmp;
    getJointCommand_planed(jointsPositionTmp);
    PointPro tmpcurrentJointPoint;
    tmpcurrentJointPoint.pointNum=0;
    tmpcurrentJointPoint.recordStatus=1;
    tmpcurrentJointPoint.jointFlag=JOINT_FLAG_JOINT;//
    tmpcurrentJointPoint.ufValue=coordinateManager->getCurrentUserIndex();
    tmpcurrentJointPoint.utValue=coordinateManager->getCurrentToolIndex();
    tmpcurrentJointPoint.positionValue.resize(jointsPositionTmp.size());
    for(int i=0;i<jointsPositionTmp.size();i++)
    {
        tmpcurrentJointPoint.positionValue[i]=jointsPositionTmp[i];
    }
    PointPro tmpcurrentCoordinatePoint;
    if(1!=coordinateManager->pointCoordinateToCartesian(tmpcurrentJointPoint,tmpcurrentCoordinatePoint))
    {
        qDebug()<<"----------error 1cdd";
        addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5059,robotId);
        return -1;
    }
    Frame positionFrameCommandOut;
    if(1!=coordinateManager->pointProToFrame_noUT(tmpcurrentCoordinatePoint,positionFrameCommandOut))
    {
        qDebug()<<"calculateCurrentPoint2 error ";
    }
    coordinateAxisArry[0]->setPositionFrameCommand_planed(positionFrameCommandOut);
    return 1;
}

int AxisGroup::setJointCommandPlaned_byPositionFrameCommandPlaned()
{
    //参考位置
    vector<double> positonInitialList;
    positonInitialList.resize(armControlAxisNumber);
    for(int i=0;i<armControlAxisNumber;i++)
    {
       positonInitialList[i]=axisArry[i]->positionCommand_planed;
    }
    string configString;
    //坐标轴数据转移到关节轴数据.
    PointPro stepPoint;
    bool isSinguratyOut;
    if(1==coordinateManager->userFrameToPointPro_JOINT(coordinateAxisArry[0]->positionFrameCommand_planed,
                            stepPoint,isSinguratyOut,configString,positonInitialList,false ))
    {
        //混合轨迹位置
        for(int j=0;j<stepPoint.positionValue.size();j++)
        {
             setPlanStartPosition(stepPoint.positionValue[j],j);
        }
    }
    else
    {
        qDebug()<<"warnning,,,setJointCommandPlaned_byPositionFrameCommandPlaned ,1!=coordinateManager->frameToPointPro_JOINT 切换关节运动会有问题 ";
        return -1;
    }

    return 1;
}

int AxisGroup::calculateCurrentFrameCommandPoint_byJointCommand(Frame &positionFrameCommandOut)
{

    PointPro tmpcurrentJointPoint;

    if(1!=calculateCurrentFrameCommandPoint_byJointCommand(tmpcurrentJointPoint))
    {
        return -1;
    }
    if(1!=coordinateManager->pointProToFrame_noUT(tmpcurrentJointPoint,positionFrameCommandOut))
    {
        qDebug()<<"calculateCurrentPoint2 error ";
        return -2;
    }

    return 1;
}

int AxisGroup::calculateCurrentFrameCommandPoint_byJointCommand(PointPro &positionCommandOut)
{
    QVector<double> jointsPositionTmp;
    getJointCommand(jointsPositionTmp);

    positionCommandOut.pointNum=0;
    positionCommandOut.recordStatus=1;
    positionCommandOut.jointFlag=JOINT_FLAG_JOINT;//
    positionCommandOut.ufValue=coordinateManager->getCurrentUserIndex();
    positionCommandOut.utValue=coordinateManager->getCurrentToolIndex();
    positionCommandOut.positionValue.resize(jointsPositionTmp.size());
    for(int i=0;i<jointsPositionTmp.size();i++)
    {
        positionCommandOut.positionValue[i]=jointsPositionTmp[i];
    }

    if(1!=coordinateManager->pointCoordinateToCartesian(positionCommandOut,positionCommandOut))
    {
        qDebug()<<"----------error 1cssb pointCoordinateToCartesian";
        addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5059,robotId);
        return -1;
    }
    return 1;
}

int AxisGroup::upDateAllFeedbackAndOffset()
{
    //模拟电机
    //motorFeedbackDataVector=motorCommandDataVector;

    //更新电机反馈值到关节值
     //axisGroupDecouple->executeReverseDecoupleToJoint(); setMotorFeedback函数里面有计算该函数了，避免重复。
     QVector<double> jointsPosition, jointsVel, jointsAcc,jointsTorque;
     if(-1==axisGroupDecouple->getFeedbackJoint(jointsPosition, jointsVel, jointsAcc,jointsTorque) )
     {
         return -1;
     }
     for(int i=0;i<totalAxisNumber;i++)
     {
         axisArry[i]->positionFeedback=jointsPosition[i];
         axisArry[i]->velFeedback=jointsVel[i];
         axisArry[i]->torqueFeedback=jointsTorque[i];
         axisArry[i]->calculateFollowOffset();
     }


     return 1;
}

int AxisGroup::getJointFeedback(QVector<double> &jointsPositionOut, QVector<double> &jointsVelOut,
                                QVector<double> &jointsAccOut, QVector<double> &jointsTorqueOut )
 {

     axisGroupDecouple->getFeedbackJoint(jointsPositionOut,jointsVelOut,jointsAccOut,jointsTorqueOut);

     return 1;
 }

double AxisGroup::getAccumulatedTime(int trajIndexIn)
{
    double tmpMax=-1;
    for(int i=0;i<totalAxisNumber;i++)
    {
        if(axisArry[i]->getAccumulatedTime()>tmpMax)
        {
             tmpMax=axisArry[i]->getAccumulatedTime(trajIndexIn);
        }

    }

    for(int i=0;i<coordinateAxisNumber;i++)
    {
        if(coordinateAxisArry[i]->getAccumulatedTime()>tmpMax)
        {
             tmpMax=coordinateAxisArry[i]->getAccumulatedTime(trajIndexIn);
        }

    }
    return tmpMax;
}

double AxisGroup::getAccumulatedTime_joint(int trajIndexIn)
{
    double tmpMax=-1;
    for(int i=0;i<totalAxisNumber;i++)
    {
        if(axisArry[i]->getAccumulatedTime()>tmpMax)
        {
             tmpMax=axisArry[i]->getAccumulatedTime(trajIndexIn);
        }
    }
    return tmpMax;
}

double AxisGroup::getAccumulatedTime_decart(int trajIndexIn)
{
    double tmpMax=-1;

    for(int i=0;i<coordinateAxisNumber;i++)
    {
        if(coordinateAxisArry[i]->getAccumulatedTime()>tmpMax)
        {
             tmpMax=coordinateAxisArry[i]->getAccumulatedTime(trajIndexIn);
        }

    }
    return tmpMax;
}

int AxisGroup::setVelCompensation(double velIn, int axisId)
{
    if(0>axisId || totalAxisNumber<=axisId)
    {
        return -1;
    }
    return axisArry[axisId]->setVelCompensation(velIn);
}

int AxisGroup::setTorqueCompensation(double torqueIn, int axisId)
{
    if(0>axisId || totalAxisNumber<=axisId)
    {
        return -1;
    }
    return axisArry[axisId]->setTorqueCompensation(torqueIn);
}

int AxisGroup::setPlanStartPosition(double positionIn, int axisId)
{
    if(0>axisId || totalAxisNumber<=axisId)
    {
        return -1;
    }
    return axisArry[axisId]->setPlanStartPosition(positionIn);
}

int AxisGroup::setJointPlanStartPosition_byPositionCommand()
{
    for(int i=0;i<totalAxisNumber;i++)
    {
        axisArry[i]->setPlanStartPosition_byPositionCommand();
    }
    return 1;
}

int AxisGroup::getJointCommand(QVector<double> &jointsPositionOut, QVector<double> &jointsVelOut,
                                QVector<double> &jointsAccOut, QVector<double> &jointsTorqueOut )
 {
     axisGroupDecouple->getCommandJoint(jointsPositionOut,jointsVelOut,jointsAccOut,jointsTorqueOut);

     return 1;
 }

int AxisGroup::getJointCommand(QVector<double> &jointsPositionOut)
{

    jointsPositionOut.resize(totalAxisNumber);
    for(int i=0;i<totalAxisNumber;i++)
    {
        jointsPositionOut[i]=axisArry[i]->positionCommand;

    }

    return 1;
}

int AxisGroup::getJointCommand_planed(QVector<double> &jointsPositionOut)
{

    jointsPositionOut.resize(totalAxisNumber);
    for(int i=0;i<totalAxisNumber;i++)
    {
        jointsPositionOut[i]=axisArry[i]->positionCommand_planed;

    }

    return 1;
}

int AxisGroup::executeDataToMotor()
{
    //轴状态检查
//    if(ALLOW_AXIS_STATUS_CHECK)
//    {
//        for(int i=0;i<totalAxisNumber;i++)
//        {
//            if(axisArry[i]->basicStatus==E_AXIS_STATUS_ERROR)
//            {
//                return -1;
//            }
//        }
//    }

    //解耦
    QVector<double> jointsPosition, jointsVel, jointsAcc , jointsTorque,jointsVelCompensation,
            jointsTorqueCompensation;
    jointsPosition.resize(totalAxisNumber);
    jointsVel.resize(totalAxisNumber);
    jointsAcc.resize(totalAxisNumber);
    jointsTorque.resize(totalAxisNumber);
    jointsVelCompensation.resize(totalAxisNumber);
    jointsTorqueCompensation.resize(totalAxisNumber);
    for(int i=0;i<totalAxisNumber;i++)
    {
//        jointsPosition[i]=axisArry[i]->positionCommand;
        jointsPosition[i]=axisArry[i]->getPositionCommand_filter();
        jointsVel[i]=axisArry[i]->velCommand;
        jointsAcc[i]=axisArry[i]->accCommand;
        jointsTorque[i]=axisArry[i]->torqueCommand;
        jointsVelCompensation[i]=axisArry[i]->getVelCompensation();
        jointsTorqueCompensation[i]=axisArry[i]->torqueCompensation;
    }
    axisGroupDecouple->setDecoupleData( jointsPosition, jointsVel, jointsAcc, jointsTorque);
    if(D_USE_VELCOMPENSATION)
    {
       axisGroupDecouple->setDecoupleData_velCompensation(jointsVelCompensation);
    }
    axisGroupDecouple->setDecoupleData_torqueCompensation(jointsTorqueCompensation);
    if(-1==axisGroupDecouple->executeDecoupleToPulse() )
    {
        return -1;
    }

    return 1;

}

int AxisGroup::asyncTrapMove(QVector<double> positionVector,double speedPercent,QVector<int> axisIdVector)
{
    //判断是否有轴的velProfile被占用
    if(0==isAllVelProfileFinished(axisIdVector))
    {
        return -1;
    }

    int validAxisSize=axisIdVector.size();


    if(0==validAxisSize)
    {
        return -1;
    }
    for(int i=0;i<validAxisSize;i++)
    {
        if(-1==axisArry[axisIdVector[i]]->trapMove(positionVector[i], speedPercent) )
        {
            return -1;
        }
    }

    return 1;
}

int AxisGroup::lineMove(Frame positionIn, double speedPercent, double &timeDurationResult,
                        int axisId,double minMixDurationIn,int cntIn, int idIn)
{
    int tmpKey= coordinateAxisArry[axisId]->lineMove(positionIn,speedPercent,
                                                timeDurationResult,minMixDurationIn);
    if(1==tmpKey)
    {
        addTrajInfo(false,timeDurationResult,cntIn,idIn);
    }
    return tmpKey;
}

int AxisGroup::lineContinuousMove_mixedCNT(QVector<Frame> positionIn, QVector<double> speedPercent,
                        QVector<double> &timeDurationResult, QVector<double> cntList,
                                           int axisId,double minMixDurationIn,QVector<int> accListIn)
{
    return coordinateAxisArry[axisId]->lineContinuousMove_mixedCNT(positionIn,speedPercent,
                                      timeDurationResult,cntList,minMixDurationIn,accListIn);
}

int AxisGroup::lineContinuousMove(QVector<Frame> positionIn, QVector<double> speedPercent
                                  , QVector<double> &timeDurationResult,double cnt,int axisId)
{
    return coordinateAxisArry[axisId]->lineContinuousMove(positionIn,speedPercent,timeDurationResult,cnt);
}


int AxisGroup::circleMove(QVector<Frame>  positionIn, double speedPercent,
                          QVector<double> &timeDurationResult,int axisId,int cntIn, int idIn)
{
    int tmpKey= coordinateAxisArry[axisId]->circleMove(positionIn,speedPercent,timeDurationResult);
    if(1==tmpKey)
    {
        addTrajInfo(false,timeDurationResult[0]+timeDurationResult[1],cntIn,idIn);
    }
    return tmpKey;
}


int AxisGroup::syncTrapMove(QVector<double> positionVector, double speedPercent, QVector<int> axisIdVector, double &timeDurationResult)
{
    //判断是否有轴的velProfile被占用
    if(0==isAllVelProfileFinished(axisIdVector))
    {
        return -1;
    }

    int validAxisSize=axisIdVector.size();


    if(0==validAxisSize)
    {
        qDebug()<<"error: 0==validAxisSize";
        return -1;
    }
    QVector<double> timeDurationVector;
    timeDurationVector.resize(validAxisSize);
    for(int i=0;i<validAxisSize;i++)
    {
        axisArry[axisIdVector[i]]->getTimeDurationTrapMove(positionVector[i],speedPercent,
                                                           timeDurationVector[i]);
        if(0<i)
        {
            if(timeDurationVector[i]>timeDurationVector[0])
            {
                timeDurationVector[0]=timeDurationVector[i];
            }
        }
    }
    for(int i=0;i<validAxisSize;i++)
    {
        if(-1==axisArry[axisIdVector[i]]->trapMoveByTime(positionVector[i],
                                                         timeDurationVector[0],speedPercent) )
        {
            qDebug()<<"error: -1==axisArry[axisIdVector[i]]->trapMoveByTime(positionVector[i], timeDurationVector[0])";
            return -1;
        }
    }

    timeDurationResult=timeDurationVector[0];
    return 1;
}

int AxisGroup::syncTrapMoveByTime(QVector<double> positionVector, double timeDuration,
                                  QVector<int> axisIdVector, double speedRatio)
{
    //判断是否有轴的velProfile被占用
    if(0==isAllVelProfileFinished(axisIdVector))
    {
        return -1;
    }

    int validAxisSize=axisIdVector.size();


    if(0==validAxisSize)
    {
        return -1;
    }
    for(int i=0;i<validAxisSize;i++)
    {
        if(-1==axisArry[axisIdVector[i]]->trapMoveByTime(positionVector[i], timeDuration,speedRatio) )
        {
            return -1;
        }
    }

    return 1;
}

int AxisGroup::asyncSplineZeroMove(QVector<double> positionVector,double speedPercent,QVector<int> axisIdVector)
{
    //判断是否有轴的velProfile被占用
    if(0==isAllVelProfileFinished(axisIdVector))
    {
        return -1;
    }

    int validAxisSize=axisIdVector.size();


    if(0==validAxisSize)
    {
        return -1;
    }
    for(int i=0;i<validAxisSize;i++)
    {
        if(-1==axisArry[axisIdVector[i]]->splineZeroMove(positionVector[i], speedPercent) )
        {
            return -1;
        }
    }

    return 1;
}
bool AxisGroup::isPositonValueChanged(QVector<double> positionVector,QVector<int> axisIdVector)
{
    int validAxisSize=axisIdVector.size();
    for(int i=0;i<validAxisSize;i++)
    {
        if(axisArry[axisIdVector[i]]->positionCommand!=positionVector[i])
        {
            return true;
        }
    }
    return false;
}

int AxisGroup::syncSplineZeroMove(bool isImprovedSpline, QVector<double> positionVector,
                                  double speedPercent, QVector<int> axisIdVector, double &timeDurationResult)
{
//    if(false==isPositonValueChanged(positionVector,axisIdVector))
//    {//无法更新行号
//        addMsg(ENUM_MSG_WARNING,"MotionServer","AxisGroup",5801,robotId);
//        return 1;
//    }

    //判断是否有轴的velProfile被占用
    if(0==isAllVelProfileFinished(axisIdVector))
    {
        return -1;
    }

    int validAxisSize=axisIdVector.size();

    //for debug
//    qDebug()<<"AxisGroup::syncSplineZeroMove speedPercent"<<speedPercent;
//    for(int i=0;i<validAxisSize;i++)
//    {

//        qDebug()<<"positionVector"<<positionVector[i];
//        qDebug()<<"axisIdVector"<<axisIdVector[i];
//    }

    if(0==validAxisSize)
    {
        qDebug()<<"error: 0==validAxisSize";
        return -1;

    }
    QVector<double> timeDurationVector;
    timeDurationVector.resize(validAxisSize);
    for(int i=0;i<validAxisSize;i++)
    {
        if(isImprovedSpline)
        {
           axisArry[axisIdVector[i]]->getTimeDurationImprovedSplineZeroMove(positionVector[i],speedPercent, timeDurationVector[i]);
        }
        else
        {
           axisArry[axisIdVector[i]]->getTimeDurationSplineZeroMove(positionVector[i],speedPercent, timeDurationVector[i]);
        }

        //for debug
//         qDebug()<<"AxisGroup::syncSplineZeroMove##################timeDurationVector[i] "<<i <<"="<<timeDurationVector[0]
//                  <<" positionVector[i]= "<<positionVector[0];
        if(0<i)
        {
            if(timeDurationVector[i]>timeDurationVector[0])
            {
                timeDurationVector[0]=timeDurationVector[i];
            }
        }
    }
    for(int i=0;i<validAxisSize;i++)
    {
        int isSucessKey;
        if(isImprovedSpline)
        {
           isSucessKey=axisArry[axisIdVector[i]]->improvedSplineZeroMoveByTime(positionVector[i], timeDurationVector[0]);
        }
        else
        {
           isSucessKey=axisArry[axisIdVector[i]]->splineZeroMoveByTime(positionVector[i], timeDurationVector[0]);
        }
        if(-1==isSucessKey )
        {
            qDebug()<<"error: -1==axisArry[axisIdVector[i]]->splineZeroMoveByTime(positionVector[i], timeDurationVector[0])";
            return -1;
        }
    }
    //for debug
//    qDebug()<<"AxisGroup::syncSplineZeroMove##################timeDurationVector[0] "<<timeDurationVector[0]<<" positionVector[0] "<<positionVector[0];

    timeDurationResult=timeDurationVector[0];
    return 1;
}

int AxisGroup::syncZeroMove_velVariable(E_VELOCITY_TYPE velocityStyleIn, QVector<double> positionVector,
                                        double speedPercent, QVector<int> axisIdVector,
                                        double &timeDurationResult, double minTimeDurationIn,
                                        int cntIn, int idIn)
{
//    if(false==isPositonValueChanged(positionVector,axisIdVector))
//    {//无法更新行号
//        addMsg(ENUM_MSG_WARNING,"MotionServer","AxisGroup",5801,robotId);
//        return 1;
//    }

    //判断是否有轴的velProfile被占用
//    if(0==isAllVelProfileFinished(axisIdVector))
//    {
////        qDebug()<<"syncZeroMove_velVariable 0==isAllVelProfileFinished(axisIdVector)";
//        return -1;
//    }

    int validAxisSize=axisIdVector.size();

    //for debug
//    qDebug()<<"AxisGroup::syncSplineZeroMove speedPercent"<<speedPercent;
//    for(int i=0;i<validAxisSize;i++)
//    {

//        qDebug()<<"positionVector"<<positionVector[i];
//        qDebug()<<"axisIdVector"<<axisIdVector[i];
//    }

    if(0==validAxisSize)
    {
        qDebug()<<"error: 0==validAxisSize";
        return -1;

    }
    QVector<double> timeDurationVector;
    timeDurationVector.resize(validAxisSize);
    for(int i=0;i<validAxisSize;i++)
    {
        switch(velocityStyleIn)
        {
        case E_VELOCITY_TYPE_SIN://bug 速度异常变换．
            axisArry[axisIdVector[i]]->getTimeDurationSinMove(positionVector[i],speedPercent, timeDurationVector[i]);
        break;
        case E_VELOCITY_TYPE_IMPROVED_SPLINE:////待完善　bug 99-100%会超过最大速度
//            axisArry[axisIdVector[i]]->getTimeDurationImprovedSplineZeroMove(positionVector[i],speedPercent, timeDurationVector[i]);
//        break;
        case E_VELOCITY_TYPE_SPLINE:
            axisArry[axisIdVector[i]]->getTimeDurationSplineZeroMove(positionVector[i],speedPercent, timeDurationVector[i]);
        break;
        default:
        {
            axisArry[axisIdVector[i]]->getTimeDurationTrapMove(positionVector[i],speedPercent, timeDurationVector[i]);
        }
        }


        //for debug
//         qDebug()<<"AxisGroup::syncSplineZeroMove##################timeDurationVector[i] "<<i <<"="<<timeDurationVector[0]
//                  <<" positionVector[i]= "<<positionVector[0];
        if(0<i)
        {
            if(timeDurationVector[i]>timeDurationVector[0])
            {
                timeDurationVector[0]=timeDurationVector[i];
            }
        }
    }

    if(timeDurationVector[0]<minTimeDurationIn)
    {
        qDebug()<<"根据过渡时间重新调整轨迹长度Time"<<timeDurationVector[0]<<"minTimeDurationIn"<<minTimeDurationIn;
        timeDurationVector[0]=minTimeDurationIn*D_MIX_ENLARGE_RATIO;
    }


    for(int i=0;i<validAxisSize;i++)
    {
        int isSucessKey;
        switch(velocityStyleIn)
        {
        case E_VELOCITY_TYPE_SIN:
            isSucessKey=axisArry[axisIdVector[i]]->sinZeroMoveByTime(positionVector[i], timeDurationVector[0]);
        break;
        case E_VELOCITY_TYPE_IMPROVED_SPLINE://待完善
//            isSucessKey=axisArry[axisIdVector[i]]->improvedSplineZeroMoveByTime(positionVector[i], timeDurationVector[0]);
//        break;
        case E_VELOCITY_TYPE_SPLINE:
            isSucessKey=axisArry[axisIdVector[i]]->splineZeroMoveByTime(positionVector[i], timeDurationVector[0]);
        break;
        default:
        {
            isSucessKey=axisArry[axisIdVector[i]]->trapMoveByTime(positionVector[i],
                                                                  timeDurationVector[0],speedPercent);
        }
        }

        if(-1==isSucessKey )
        {
            qDebug()<<"error: -1==axisArry[axisIdVector[i]]->splineZeroMoveByTime(positionVector[i], timeDurationVector[0])";
            return -1;
        }
    }
    //for debug
//    qDebug()<<"AxisGroup::syncSplineZeroMove##################timeDurationVector[0] "<<timeDurationVector[0]<<" positionVector[0] "<<positionVector[0];

    timeDurationResult=timeDurationVector[0];
    addTrajInfo(true,timeDurationResult,cntIn,idIn);
    return 1;
}

int AxisGroup::syncSplineZeroMoveByTime(QVector<double> positionVector,double timeDuration,QVector<int> axisIdVector)
{
    //判断是否有轴的velProfile被占用
    if(0==isAllVelProfileFinished(axisIdVector))
    {
        return -1;
    }

    int validAxisSize=axisIdVector.size();


    if(0==validAxisSize)
    {
        return -1;
    }
    for(int i=0;i<validAxisSize;i++)
    {
        if(-1==axisArry[axisIdVector[i]]->splineZeroMoveByTime(positionVector[i], timeDuration) )
        {
            return -1;
        }
    }

    return 1;
}

int AxisGroup::syncSplineContinueMove(QVector<double> positionVector,QVector<double> velVector
                                      ,QVector<double> accVector,QVector<int> axisIdVector)
{
    //判断是否有轴的velProfile被占用
    if(0==isAllVelProfileFinished(axisIdVector))
    {
        return -1;
    }

    int validAxisSize=axisIdVector.size();

    if(0==validAxisSize)
    {
        return -1;
    }
    QVector<double> timeDurationVector;
    timeDurationVector.resize(validAxisSize);
    for(int i=0;i<validAxisSize;i++)
    {
        axisArry[axisIdVector[i]]->getTimeDurationSplineContinueMove(positionVector[i], velVector[i], timeDurationVector[i]);
        if(0<i)
        {
            if(timeDurationVector[i]>timeDurationVector[0])
            {
                timeDurationVector[0]=timeDurationVector[i];
            }
        }
    }
    for(int i=0;i<validAxisSize;i++)
    {
        if(-1==axisArry[axisIdVector[i]]->splineContinueMoveByTime(positionVector[i], velVector[i],accVector[i],timeDurationVector[0]) )
        {
            return -1;
        }

    }

    return 1;
}

int AxisGroup::splineContinueMoveMultiPoint(QVector<QVector<VelPoint> > velPointArry, QVector<int> axisIdVector
                                            , int isLastPointMove, QVector<double> & timeDurationListResult)
{
    //判断是否有轴的velProfile被占用
    if(0==isAllVelProfileFinished(axisIdVector))
    {
        return -1;
    }

    int validAxisSize=axisIdVector.size();

    if(0==validAxisSize || velPointArry.size()==0)
    {
        return -1;
    }
    int pointSize=velPointArry[0].size();

    QVector<QVector<double> > timeDurationVector;
    timeDurationVector.resize(validAxisSize);
    for(int i=0;i<validAxisSize;i++)
    {
      timeDurationVector[i].resize(pointSize);
    }

    timeDurationListResult.resize(pointSize);
    //求同步时间
    for(int j=0;j<pointSize;j++)
    {
        //情况１：根据速率百分比求时间
        if(0>velPointArry[0][j].timeDuration)
        {
            for(int i=0;i<validAxisSize;i++)
            {
                axisArry[axisIdVector[i]]->getTimeDurationSplineContinueMove(velPointArry[i][j].positionIn,
                                                                             velPointArry[i][j].velIn, timeDurationVector[i][j]);
                if(0<i)
                {
                    if(timeDurationVector[i][j]>timeDurationVector[0][j])
                    {
                        timeDurationVector[0][j]=timeDurationVector[i][j];
                    }
                }
            }
            for(int i=0;i<validAxisSize;i++)
            {
                  velPointArry[i][j].timeDuration = timeDurationVector[0][j];
            }
            timeDurationListResult[j]=timeDurationVector[0][j];
        }
        else
        {
            //情况２：直接根据传入时间
            timeDurationListResult[j]=velPointArry[0][j].timeDuration;
        }



    }

    //估算进过点速度
    for(int j=0;j<pointSize;j++)
    {

        for(int i=0;i<validAxisSize;i++)
        {
            if(0==j)
            {
                velPointArry[i][j].velIn=(velPointArry[i][j].positionIn-axisArry[axisIdVector[i]]->positionCommand)
                                            /velPointArry[i][j].timeDuration;
            }
            else if(pointSize-1>j)
            {
                velPointArry[i][j].velIn=(velPointArry[i][j].positionIn-velPointArry[i][j-1].positionIn)
                                            /velPointArry[i][j].timeDuration;
            }
            //最后点静止判断
            if(pointSize-1==j && isLastPointMove==0)
            {
                velPointArry[i][j].velIn=0;
            }
            else if(pointSize-1==j && isLastPointMove==1)
            {
                velPointArry[i][j].velIn=(velPointArry[i][j].positionIn-velPointArry[i][j-1].positionIn)
                                            /velPointArry[i][j].timeDuration;
            }
//            qDebug()<<" vel asix id="<<i<<"point id="<<j <<velPointArry[i][j].velIn;

        }

    }

    //调用轴
    for(int i=0;i<validAxisSize;i++)
    {
        if(-1==axisArry[axisIdVector[i]]->splineContinueMoveMultiPoint(velPointArry[i]))
        {
            qDebug()<<"error: splineContinueMoveMultiPoint -1==axisArry[axisIdVector[i]]->splineContinueMoveMultiPoint(velPointArry[i])";
            return -1;
        }

    }

    //求累计轨迹时间
    QVector<double>  tmpTimeList;
    tmpTimeList.resize(pointSize);
    for(int i=0;i<pointSize;i++)
    {
        tmpTimeList[i]=0;
        for(int j=0;j<=i;j++)
        {

            tmpTimeList[i]+=timeDurationListResult[j];
        }
    }
    timeDurationListResult=tmpTimeList;

    return 1;
}

int AxisGroup::syncSplineContinueMoveByTime(QVector<double> positionVector, QVector<double> velVector
                                            , QVector<double> accVector, double timeDuration, QVector<int> axisIdVector)
{
    //判断是否有轴的velProfile被占用
    if(0==isAllVelProfileFinished(axisIdVector))
    {
        return -1;
    }

    int validAxisSize=axisIdVector.size();


    if(0==validAxisSize)
    {
        return -1;
    }
    for(int i=0;i<validAxisSize;i++)
    {
        if(-1==axisArry[axisIdVector[i]]->splineContinueMoveByTime(positionVector[i],velVector[i],accVector[i],timeDuration) )
        {
            return -1;
        }
    }

    return 1;
}

int AxisGroup::writeDataToFile()
{
//    axisGroupDecouple->dataAnalysizerForJointCommand->writeDataQueueToFile("axisGroupJointCommand.csv");
//    axisGroupDecouple->dataAnalysizerForJointFeedback->writeDataQueueToFile("axisGroupJointFeedback.csv");
//    axisGroupDecouple->dataAnalysizerForMotorCommand->writeDataQueueToFile("axisGroupMotorCommand.csv");
//    axisGroupDecouple->dataAnalysizerForMotorFeedback->writeDataQueueToFile("axisGroupMotorFeedback.csv");
}


QVector<MotorData> AxisGroup::getMotorCommandDataVector()
{
    return axisGroupDecouple->getMotorDataCommandVector();
}

QVector<MotorData> AxisGroup::getMotorFeedbackDataVector()
{
    return axisGroupDecouple->getMotorDataFeedbackVector();
}

int AxisGroup::setMotorFeedbackDataVector(QVector<MotorData> motorFeedbackIn)
 {
   axisGroupDecouple->setMotorFeedbackDataVector(motorFeedbackIn);
   //axisGroupDecouple->executeReverseDecoupleToJoint();重复了。
   QVector<double> jointsPosition,jointsVel,jointAcc, jointTorque;
   jointsPosition.resize(totalAxisNumber);
   jointsVel.resize(totalAxisNumber);
   jointAcc.resize(totalAxisNumber);
   jointTorque.resize(totalAxisNumber);
   axisGroupDecouple->getFeedbackJoint(jointsPosition,jointsVel,jointAcc, jointTorque);
   for(int i=0;i<totalAxisNumber;i++)
   {
       axisArry[i]->positionFeedback=jointsPosition[i];
//       if(1==i)
//       {
//              qDebug()<<"a67666xisArry[i]->positionFeedback"<<axisArry[i]->positionFeedback;
//       }

       axisArry[i]->velFeedback=jointsVel[i];
       axisArry[i]->torqueFeedback=jointTorque[i];
   }
   return 1;
 }

int AxisGroup::isTimeVelProfileEmpty()
{
    return timeAxis[0]->isTrajComposeEmpty();
}

 int AxisGroup::isTimeVelProfileEmpty(int timeAxisIndex)
 {
     return timeAxis[timeAxisIndex]->isTrajComposeEmpty();
 }

//int AxisGroup::setAxisPositionCommand(std::vector<float> positionValue)
//{
//    for(int i=0;i<totalAxisNumber;i++)
//    {
//        axisArry[i]->positionCommand=positionValue[i];

//    }
//    return 1;
//}

int AxisGroup::setCheckMode(E_CHECK_MODE modeIn)
{
    for(int i=0;i<totalAxisNumber;i++)
    {
        axisArry[i]->setCheckMode(modeIn);
    }
    for(int i=0;i<coordinateAxisNumber;i++)
    {
        coordinateAxisArry[i]->setCheckMode(modeIn);
    }
    return 1;
}

int AxisGroup::getFeedbackJoint(QVector<double> &jointsPosition, QVector<double> &jointsVel, QVector<double> &jointAcc, QVector<double> &jointTorque)
{
    axisGroupDecouple->getFeedbackJoint(jointsPosition,jointsVel,jointAcc,jointTorque);
    return 1;

}



bool AxisGroup::isAxisZeroOk()
{


    return zeroManagement->isAxisZeroOk();


}

bool AxisGroup::isAxisZeroOk(int axisId)
{

    return zeroManagement->isAxisZeroOk(axisId);


}

int AxisGroup::getAxisZeroState(int axisId)
{
    return zeroManagement->getAxisZeroState(axisId);
}

QVector<int> AxisGroup::getAbsoluteZeroMotorValue()
{
    return zeroManagement->getAbsoluteZeroMotorValue();
}

QVector<double> AxisGroup::getJointMechanicalZeroValue()
{
    return zeroManagement->getJointMechanicalZeroValue();
}

int AxisGroup::getCalibrateInformation(S_AXISGROUP_ZERO_INFORMATION &zeroInformationOut)
{
    return zeroManagement->getCalibrateInformation(zeroInformationOut);
}

int AxisGroup::axisLosePower()
{
    return zeroManagement->axisLosePower();
}

int AxisGroup::axisLosePower(int axisId)
{
    return zeroManagement->axisLosePower(axisId);
}

int AxisGroup::calibrateRobotByOffset(std::vector<double> offsetIn)
{
    return zeroManagement->calibrateRobotByOffset(offsetIn);
}

int AxisGroup::validateZeroData()
{

    return zeroManagement->validateZeroData();
}

int AxisGroup::calibrateRobot()
{
    return zeroManagement->calibrateRobot();


}

int AxisGroup::calibrateRobot(int axisId)
{
   return zeroManagement->calibrateRobot(axisId);
}


int AxisGroup::addMsg(int messageLevel, string componentName, string messageType, int messageCode, int _robotId,
                                     int parameter1, int parameter2, int parameter3, int parameter4)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageCode = messageCode;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = _robotId;

    string infomationStr;

    switch(messageCode)
    {
    case 5802:
    {
       string str1;
       str1 = QObject::tr("机器人(ID:").toStdString() +
               QString::number(_robotId).toStdString() +
               QObject::tr(")").toStdString() +
               QObject::tr("目标点超过时间%1毫秒未到达目标点允许误差范围！").arg(parameter1).toStdString();
       infomationStr = str1;
       break;
    }
    case 5801:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")").toStdString() +
                QObject::tr("轴运动为空,当前位置和目标位置没有任何差别!").toStdString();
        infomationStr = str1;
        break;
     }
    case 5800:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(_robotId).toStdString() +
                QObject::tr(")第").toStdString() +
                QString::number(parameter1+1).toStdString() +
                QObject::tr("轴状态错误，不能运行").toStdString();
        infomationStr = str1;
        break;
     }




        default:
        {
            qDebug()<<"Coordinate::addMsg====not match error code";
            break;
        }
    }

    tmpMsg.MessageInformation = infomationStr;

    #ifndef MOTION_SERVER_UNIT_TEST
    motionMessage->addMsg(tmpMsg);
    #endif
}
